Method for kalman filter state estimation in bilinear systems

ABSTRACT

The method for Kalman filter state estimation in bilinear systems provides for state estimation in dynamic systems, and is a bilinear extension of the Kalman filter and the Kalman smoother. The method for Kalman filter state estimation in bilinear systems introduces a nonlinear state equation coupled with a linear measurements equation. The specific nonlinearity is of the bilinear form, depending upon the system dynamics.

BACKGROUND OF THE INVENTION

1. Field of the Invention

The present invention relates to state estimation in dynamic systems,and particularly to a bilinear extension of the Kalman filter and theKalman smoother.

2. Description of the Related Art

The Kalman filter is an estimator for the linear-quadratic problem,which is the problem of estimating the instantaneous state of a lineardynamic system perturbed by white noise using measurements that arelinearly related to the state, but are also corrupted by white noise.The Kalman filter produces values that tend to be closer to the truevalues of the measurements and their associated calculated values bypredicting an estimate of uncertainty of the predicted value via aweighted average of the predicted and measured values. The Kalman filteris also an algorithm for efficient performance of the exact inference ina linear state space model that has some statistical properties. Theresulting estimator is statistically optimal with respect to somequadratic function of the estimation error.

Viewed mathematically, the Kalman filter is a set of equations thatprovides an efficient recursive solution of the least square method. Itprovides estimates of the past, present, and future states, and it cando so when the precise nature of the system model is unknown.

The Kalman estimator may also be adapted as a smoother. A smootherestimates the state of a system at time k, using measurements madebefore and after time k. The accuracy of a smoother is generallysuperior to that of a filter, since it uses more measurements for itsestimate. The Kalman smoother can be derived from the Kalman filtermodel.

The general derivation methodology for the Kalman smoother uses theKalman filter for measurements up to (each) time k that the state is tobe estimated, combined with another algorithm also derived from theKalman filter for the measurements beyond that time. This secondalgorithm for the smoother can be derived by running the Kalman filterbackward from the last measurement to just past k, and then optimallycombining the two independent estimates (forward and backward) of thestate at time k based on the two independent sets of measurements.

In the following, the Kalman filter for a linear-discrete Gaussian statespace model is considered. Initially, the nonlinear Kalman filter isexamined. A discrete-time linear state space model within a dynamicalsystem may be represented as:

x _(k+1) =Ax _(k) +w _(k),  (1)

with measurements

y _(k) =Cx _(k) +v _(k),  (2)

where x_(k)ε

^(n) is the system state vector at time k (i.e., x_(k) represents ann-dimensional real vector), A ε

^(n×n) is the transition matrix (an n×n matrix), y_(k)ε

^(p) is the corresponding measurement vector at time k, C ε

^(p×n) is the observation (or “measurement”) matrix (a p×1 matrix),w_(k)ε

^(n) is the dynamical (or system) noise at time k, and v_(k)ε

^(p) is the observation (or measurement) noise at time k.

w_(k) and v_(k) are both uncorrelated, white and Gaussian, with zeromean and covariance Q and R, respectively. In other words, w_(k)˜N(0,Q); v_(k)˜N(0, R); E(w_(k)w_(l) ^(T))=Q for k=1 and E(w_(k) w_(l)^(T))=0 for k≠1; E(v_(k)v_(l) ^(T))=R for k=1 and E(v_(k)v_(l) ^(T))=0for k≠1; and E(w_(i)v_(j) ^(T))=0 for ∀i, j, where E represents theexpectation.

From the above, for the Kalman filter, the predicted state estimate (apriori) is given by equation (1) as x_(k+1)=Ax_(k)+w_(k), and thepredicted (a priori) estimate covariance would be given by P_(k)^(f)=AP_(f) ^(f)A^(T)+Q, wherein the superscript “f” represents theforecast (e.g., x_(k) ^(f) represents the forecast state of x_(k)). Theupdated (a posteriori) state estimate is given by x_(k) ^(a)=x_(k)^(f)+K_(k)(y_(k)−Cx_(k) ^(f)), where the superscript “a” represents theestimation (e.g., x_(k) ^(a) is the estimation of x_(k)) and K_(k) isthe optimal Kalman gain, given by K_(k)=P_(k) ^(f)C^(T)(CP_(k)^(f)C^(T)+R)⁻¹. The updated (a posteriori) estimate covariance is givenby P_(k) ^(a)=P_(k) ^(f)−K_(k)CP_(k) ^(f).

With regard to the Kalman smoother, x_(k) is obtained based on datasamples y₁, . . . , y_(t), where k≦t (denoted as x_(k) ^(t)). Suchestimators are generally referred to as “smoothers” since a time plot ofthe sequence {x_(k) ^(t): k=1, . . . , t} is typically smoother than theforecasts {x_(k) ^(k-1): k=1, . . . , t} or the filter {x_(k) ^(k): k=1,. . . , t}. Thus, smoothing implies that each estimated value is afunction of the past, present and future, whereas the filter estimatordepends on the past and the present. The forecast depends on only thepast. For the Kalman smoother, the updated estimate covariance is givenby P_(k) ^(t)=P_(k) ^(a)+J_(k)└P_(k+1) ^(t)−P_(k+1) ^(f)┘J_(k) ^(T),where J_(k)=P_(k) ^(a)A^(T)(P_(k+1) ^(f))⁻¹. The lag-one covariancesmoother is a type of smoother problem, which is a set of recursions forobtaining P_(k+1,k) ^(t). For the lag-one covariance smoother, P_(k,k−1)^(t)=P_(k) ^(a)J_(k−1) ^(T)+J_(k)└P_(k+1,k) ^(t)−AP_(k) ^(a)┘J_(k−1)^(T).

The Kalman filter is an algorithm, commonly used since the 1960s, forimproving vehicle navigation (among other applications, althoughaerospace is typical), that yields an optimized estimate of the system'sstate (e.g., position and velocity). The algorithm works recursively inreal time on streams of noisy input observation data (typically, sensormeasurements) and filters out errors using a least-squares curve-fitoptimized with a mathematical prediction of the future state generatedthrough a modeling of the system's physical characteristics. The modelestimate is compared to the observation and this difference is scaled bythe Kalman gain, which is then fed back as an input into the model forthe purpose of improving subsequent predictions. The gain can be “tuned”for improved performance. With a high gain, the filter follows theobservations more closely. With a low gain, the filter follows the modelpredictions more closely. This method produces estimates that tend to becloser to the true unknown values than those that would be based on asingle measurement alone or the model predictions alone.

The Kalman filter has many applications in technology, and is anessential part of space and military technology development. A verysimple example (and perhaps the most commonly used type of Kalmanfilter) is the phase-locked loop, which is now ubiquitous in FM radiosand most electronic communications equipment. Another common applicationis with guidance, navigation and control of vehicles, like aircraft orspacecraft. Sensors are used to make measurements of the vehicle's state(its position and velocity at the time of the measurement), but suchmeasurements are intermittent and have significant stretches of timebetween measurements. Also, the measurements are corrupted with acertain amount of error, including noise.

The Kalman Filter algorithm is an optimized method for determining thebest estimation of the vehicle's state. The basic concept is similar tosimple mathematical curve fitting of data points using a least-squaresapproximation (where the deviation is squared so that negative errorswill not cancel out positive ones) and enables predictions of the stateinto future time steps. The most basic concepts of the filter arerelated to interpolation and extrapolation, where data estimates arefilled in between given points and the latter involves data estimatesbeing extended beyond the given set (as with future estimates).

In each time step, the Kalman filter produces estimates of the trueunknown values, along with their uncertainties. Once the outcome of thenext measurement is observed, these estimates are updated using aweighted average, with more weight being given to estimates with loweruncertainty. From a theoretical standpoint, the main assumption of theKalman filter is that the underlying system is a linear dynamical systemand that all error terms and measurements have a Gaussian distribution(often a multivariate Gaussian distribution). Extensions andgeneralizations to the method have also been developed. The underlyingmodel is a Bayesian model similar to a hidden Markov model, but wherethe state space of the latent variables is continuous and where alllatent and observed variables have Gaussian distributions.

The Kalman filter may be viewed as a type of “observer,” that helps toestimate the state variables of a dynamical system. For example, ingeneral, individual state variables of a dynamic system cannot bedetermined exactly by direct measurements. Rather, measurementsassociated with the state variables, e.g., sensor data in a controlsystem, may be a function of the state variables. In addition, suchmeasurements are generally subject to measurement error, which, in oneembodiment, may be modeled as random noise. Further, the system itselfmay also be subjected to random or other disturbances. The Kalman filterestimates the state variables based on the noisy measurements.

In FIG. 8, a simplified system 200 is illustrated. The system 200includes one or more sensors 202 that provide data to an electronicstate observer 100. The state observer 100 outputs a state (e.g., a realvalued array of state variables) to a control system 206, which outputssignals to one or more controls 208 based on the state output by thestate observer 100. As an example, the system 200 may be a controlsystem of an aircraft or spacecraft. In such a system, the sensors 202may include, for example, one or more of attitude, velocity, position,altitude, etc., while the controls 208 may include control surfaces,engine controls, etc. The state observer 100 is the Kalman filter,having a corresponding covariance matrix for which positive definitenessis to be maintained in order to maintain filter stability. In thisexample, the control system 206 receives aircraft state data from theKalman filter 100, which uses data from the sensors 202 to update itsestimate of one or more state variables associated with the aircraft.The control system 206 generates control signals to the controls 208 ofthe aircraft to achieve a selected control operation.

FIG. 9 illustrates the Kalman filter algorithm, using the “observer”view noted above. At step 10, the method begins with the system 200generating an observer, e.g., a Kalman filter, for estimation of atleast one state variable (contained within the state vector) associatedwith the system. The Kalman filter 100 provides periodically updatedstate variables given a sequence of observations (i.e., sensor data)about those state variables. The data from the sensors 202 generallyincludes some error or noise, such as measurement errors that can dependon the sensor, the operational environment, the nature of the measuredvalue, etc.

The Kalman filter helps to remove the effects of the errors to estimatethe state variables of the system. The Kalman filter is a recursiveestimator or state observer. Hence, the estimated state from theprevious step and a new sensor measurement are used to compute theestimate for the current state. The Kalman filter is used in two steps,i.e., a prediction step and an update step. The prediction step uses thestate estimate from the previous time step to produce an estimate of thestate at the current time step. In the update step, sensor measurementinformation at the current time step is used to refine this predictionto arrive at a new state estimate, again for the current time step. Thestate observer 100 is represented by the Kalman filter defined above.

In step 12, the state observer 100 receives the data from the sensors202. Next, at step 14, the observer calculates the initial estimates ofthe state vector x_(k−1) and the covariance P_(k−1). At step 16, thestate vector and covariance are projected ahead by one time increment,as x_(k+1)=Ax_(k)+w_(k) and P_(k) ^(f)=AP_(f) ^(f)A^(T)+Q, respectively.At step 18, the Kalman gain is computed as K_(k)=P_(k) ^(f)C^(T)(CP_(k)^(f)C^(T)+R)⁻¹. At step 20, a new set of measurements y_(k) are takenfrom sensors 202 to further update the state vector and covariance asx_(k) ^(a)=x_(k) ^(f)+K_(k)(y_(k)−Cx_(k) ^(f)) and P_(k) ^(a)=P_(k)^(f)−K_(k)CP_(k) ^(f), respectively. These updated estimates are thenfed back to step 16 for incremental projection by the next timeincrement.

With regard to identification of the linear Gaussian state-space model,“system identification” is a general term to describe the mathematicaltools and algorithms that build dynamical models from measurement data.System identification plays an important role in uncertain dynamicalsystems. The Expectation-Maximization (EM) algorithm is a broadlyapplicable approach to the iterative computation of the maximumlikelihood (ML) estimates, which are useful in a variety of incompletedata problems. On each iteration of the EM algorithm, there are twosteps, the Expectation step (E-step) and the Maximization step (M-step).The EM algorithm is a technique that can be use to estimate theparameters after filling in the initial values for the missing data. Thelatter are then updated by their predicted values using these initialparameter estimates. The parameters are then re-estimated, and so on,proceeding iteratively until convergence. This technique is generallyconsidered to be intuitive and natural.

The Kalman filter and the Kalman smoother are the basic tools forcalculating the expectation in the E-step. The discrete-time linearstate space model within a dynamical system is given above in equations(1) and (2). In this model, the initial state vector x₀ is assumed to bea Gaussian random vector with a mean μ and a covariance V, where x₀˜N(μ,V).

The maximum likelihood estimator for the parameters in this model isgiven by application of the EM algorithm. Using the Kalman filter andthe Kalman smoother, x_(k) ^(t)=E_(t)(x_(k))=E(x_(k)|{y}₁ ^(t)) (where Erepresents the expectation); P_(k) ^(t)=Cov(x_(k)|{y}₁ ^(t)), (where Covrepresents the covariance); and P_(k,k−1) ^(t)=Cov(x_(k),x_(k−1)|{y}₁^(t)). For the M-step, the parameters of the system (A, C, Q, R and μ)are collected in a vector α={A, C, Q, R, μ}. To estimate theseparameters, the corresponding partial derivatives of the expectedlog-likelihood function are taken, and then set to zero. Thus, α_(i) isupdated as α_(i+1). In other words, α_(i+1)={A(i+1), C(i+1), Q(i+1),R(i+1), μ(i+1)} is obtained by maximizing the log likelihood functionwith respect to each parameter.

The value of μ is simply given by μ=x₀ ^(t)=E_(t)(x₀). A is expressed asA(i+1)=γβ⁻¹, where

$\gamma = {{\sum\limits_{k = 0}^{t - 1}{\left( {P_{{k + 1},k}^{t} + {x_{k + 1}^{t}\left( x_{k}^{t} \right)}^{T}} \right)\mspace{14mu} {and}\mspace{14mu} \beta}} = {\sum\limits_{k = 0}^{t - 1}{\left( {P_{k}^{t} + {x_{k}^{t}\left( x_{k}^{t} \right)}^{T}} \right).}}}$

C is given by C(i+1)=εβ⁻¹, where

$ɛ = {\sum\limits_{k = 0}^{t - 1}{{E_{t}\left( {y_{k}x_{k}^{T}} \right)}.}}$

Q is given by Q(i+1)=t⁻¹(σ−γβ⁻¹γ^(T)), where

$\sigma = {\sum\limits_{k = 0}^{t - 1}{\left( {P_{k}^{t} + {x_{k}^{t}x_{k}^{t^{T}}}} \right).}}$

Lastly, R is given by R(i+1)=t⁻¹(ξ−εβ−1ε^(T)), where

$\xi = {\sum\limits_{k = 0}^{t - 1}{{E_{t}\left( {y_{k}y_{k}^{T}} \right)}.}}$

In order to identify the parameters of the state-space model using theEM algorithm to obtain the maximum likelihood estimation vector{circumflex over (α)}, first the initial values of A(0), C(0), Q(0),R(0) and μ(0) are selected, and then x_(k) ^(t), P_(k) ^(t) andP_(k,k−1) ^(t) are computed using the Kalman smoother. The conditionalexpectation of the log likelihood function is then calculated, and then{A(i+1),C(i+1),Q(i+1),R(i+1), μ(i+1)} are computed to find the nextiterative estimated parameters that maximize the conditional expectationof the log likelihood. These new parameters are then inserted back intothe state-space model, and computed using the Kalman smoother. Thesesteps are repeated until the log likelihood converges.

When the dynamical system and measurements system of the state-spacemodel are linear, the Kalman filter is the optimal estimator. However,in most “real world” processes of interest, the dynamical and/or themeasurements systems are nonlinear. Thus, a suitable extension of theKalman filter is needed. Both the Lorenz-96 model and the Lotka-Volterramodel, which are used for atmospheric dynamics, biology, chemistry, andcontrol systems, are examples of bilinear models. Generally, bilinearsystems are nonlinear systems that are of great interest, as theyrepresent a wide variety of important physical processes. Bilinearsystems can also be used in approximation or alternate representationsfor a range of other nonlinear systems. Further, bilinear models may beused to model nonlinear processes in signal, image and communicationsystems. Bilinear systems may be represented as state-space models.However, bilinear models do not work with the traditional Kalman filter.It would, therefore, be desirable to extend the Kalman filter and Kalmansmoother to be able to handle such bilinear systems.

Thus, a method for Kalman filter state estimation in bilinear systemssolving the aforementioned problems is desired.

SUMMARY OF THE INVENTION

The present invention relates to state estimation in dynamic systems,and particularly to a bilinear extension of the Kalman filter and theKalman smoother. The method for Kalman filter state estimation inbilinear systems introduces a nonlinear state equation coupled with alinear measurements equation. The specific nonlinearity is of thebilinear form, depending upon the system dynamics.

The method for Kalman filter state estimation in bilinear systemsincludes the steps of: (a) generating an observer in an electronicdevice for estimating a state vector x_(k), associated with a bilinearsystem, where the observer defines a covariance matrix P_(k)statistically relating the state vector x_(k), where k represents aninteger time increment, and the bilinear system is defined byx_(k+1)=Ax_(k)+B(x_(k)

x_(k))+w_(k), where A is a transition matrix defined by the bilinearsystem, B is a measurement matrix defined by parameters of the bilinearsystem, and w_(k) is system noise at time k; (b) receiving sensor datay_(k) at the electronic device from a sensor, the sensor data beingassociated with the state vector x_(k) as y_(k)=Cx_(k)+v_(k), where C isa measurement matrix defined by the parameters of the bilinear systemand v_(k) is measurement noise at time k; (e) generating initialestimates of the state vector and the covariance matrix from the sensordata and storing the initial estimates of the state vector and thecovariance matrix in non-transitory computer readable memory of theelectronic device; (d) calculating a projected state vector x_(k+1) ^(k)representing the state vector projected ahead by one time increment asx_(k+1) ^(l)=Ax_(k) ^(k)+Bz_(k) ^(k) and storing the projected statevector x_(k+1) ^(k) in the non-transitory computer readable memory ofthe electronic device, where z_(k)=x_(k)

x_(k) and z_(k) ^(k)=E_(k) (z_(k)); (e) calculating a projectedcovariance matrix P_(k+) ¹ representing the covariance matrix projectedahead by one time increment as P_(k+1) ^(k)=AP_(k) ^(k)A^(T)+A{umlautover (P)}_(k) ^(k)B^(T)+B({umlaut over (P)}_(k) ^(k))^(T)A^(T)+B{dotover (P)}_(k) ^(k)B^(T)+Q, where Q represents a covariance of the systemnoise w_(k), and storing the projected covariance matrix P_(k+1) ^(k) inthe non-transitory computer readable memory of the electronic device;(f) calculating a Kalman gain K_(k+1) as K_(k+1)=P_(k+1)^(k)C^(T)(CP_(k+1) ^(k)C^(T)+R)⁻¹, where R is a covariance of themeasurement noise v_(k), and storing the Kalman gain K_(k+1) in thenon-transitory computer readable memory of the electronic device; (g)receiving a new set of measurement data y_(k) from the sensor andupdating the state vector and the covariance matrix based upon the newset of measurement data as x_(k+1) ^(k+1)=K_(k+1)^(k)+K_(k+1)(y_(k)−Cx₊₁ ^(k)) and P₊₁ ^(k+)=(I−K_(k+1)C)P_(k+1) ^(k),respectively, wherein x_(k+1) ^(k+1) represents the updated state vectorfor time (k+1) and P_(k+1) ^(k+1) represents the updated covariancematrix for time (k+1), where I represents the identity matrix, andstoring the updated state vector and the updated covariance matrix inthe non-transitory computer readable memory of the electronic device;and (h) returning to steps (d) and (e) with the updated state vector andthe updated covariance matrix, respectively.

These and other features of the present invention will become readilyapparent upon further review of the following specification anddrawings.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a block diagram illustrating an overview of a system forperforming a method for Kalman filter state estimation in bilinearsystems according to the present invention.

FIG. 2 is a graph illustrating calculated error between actual statesand states estimated by the method for Kalman filter state estimation inbilinear systems according to the present invention.

FIG. 3 is a graph illustrating calculated error between actual statesand states estimated by a Kalman smoother extension of the method forKalman filter state estimation in bilinear systems according to thepresent invention.

FIG. 4 is a graph illustrating calculated error using parametersestimated by the method for Kalman filter state estimation in bilinearsystems according to the present invention produced via the EMalgorithm.

FIG. 5 is a graph illustrating calculated error between simulated statesand states estimated by the method for Kalman filter state estimation inbilinear system according to the present inventions.

FIG. 6 is a graph illustrating calculated error between simulated statesand states estimated by the ensemble Kalman filter algorithm forpurposes of comparison with FIG. 5.

FIG. 8 is block diagram illustrating a system overview forimplementation of a method for Kalman filter state estimation accordingto the prior art.

FIG. 9 is a flow chart illustrating method steps of the Kalman filteralgorithm according to the prior art.

Similar reference characters denote corresponding features consistentlythroughout the attached drawings.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

The discrete-time linear state space model within a dynamical system isrepresented by equations (1) and (2) above. The bilinear Gaussiandiscrete state space model is a variant on equations (1) and (2) and isrepresented by:

x _(k+1) =Ax _(k) +B(x _(k)

x _(k))+w _(k),  (3)

with measurements

y _(k) =Cx _(k) +v _(k),  (4)

where x_(k)ε

^(n) is the system state vector at time k (i.e., x_(k) represents ann-dimensional real vector), A ⊖

^(n×n) is the transition matrix (an n×n matrix), y_(k)ε

^(p) is the corresponding measurement vector at time k, B ε

^(n×[n(n+1)/2]) and C ε

^(p×n) are the observation (or “measurement”) matrices (i.e., theparameters of the model), w_(k)ε

^(n) is the dynamical (or system) noise at time k, and v_(k)ε

^(p) is the observation (or measurement) noise at time k.

w_(k) and v_(k) are both uncorrelated, white and Gaussian, with zeromean and covariance Q and R, respectively. In other words, w_(k)˜N(0,Q); v_(k)˜N(0, R); E(w_(k)w_(l) ^(T))=Q for k=1 and E(w_(k)w_(l) ^(T))=0for k≠1, E(v_(k)v_(l) ^(T))=R for k=1 and E(v_(k)v_(l) ^(T))=0 for k≠1;and E(w_(k)v_(j) ^(t))=0, where E represents the expectation and (x_(k)

x_(k)) is the Kronecker product of the state x_(k) with itself.

Due to the nonlinearity in the bilinear model, the estimation isperformed as a set of differential equations. For the bilinear statespace model of equations (3) and (4), the state vector and estimator arerespectively given as:

x _(k+1) ^(k) =Ax _(k) ^(k) +Bz _(k) ^(k)  (5)

P _(k+1) ^(k) =AP _(k) ^(k) A ^(T) A{umlaut over (P)} _(k) ^(k) B ^(T)+B({umlaut over (P)} _(k) ^(k))^(T) A ^(T) +B{dot over (P)} _(k) ^(k) B^(T) +Q,  (6)

where z_(k)=x_(k)

x_(k) and z_(k) ^(k)=E_(k)(z_(k)), where E represents the expectation,and

x _(k+1) ^(k+1) =x _(k+1) ^(k) +K _(k+1)(y _(k) −Cx _(k+1) ^(k))  (7)

P _(k+1) ^(k+1)=(I−K _(k+1) C)P _(k+1) ^(k),  (8)

where I represents the identity matrix and K_(k) is the optimal Kalmangain. In the above, x_(k) ^(t)=E{x_(k)|{y}_(l) ^(t)}=E_(t)x_(t)), P_(k)^(t)=E{(x_(k)−x_(k) ^(t))(x_(k)−x_(k) ^(t))^(T)}, z_(k)^(t)=E{z_(k)|{y}_(l) ^(t)}=E_(t)(z_(t)), {dot over (P)}_(k)^(t)=E{(z_(k)−z_(k) ^(t))(z_(k)−z_(k) ^(t))^(T)}, and {umlaut over(P)}_(k) ^(t)=E_(t){(x_(k)−x_(k) ^(t))(z_(k)−z_(k) ^(t))^(T)}, where1≦k≦t, 1≦t≦n, and {y}_(l) ^(t) is the measurement sequence; i.e.,{y}_(l) ^(t)={y_(j), . . . , y_(t)}.

Applying a Taylor expansion to z_(k) yields

$\begin{matrix}{{{z_{k} = {z_{k}^{j} + {{f^{\prime}\left( x_{k}^{j} \right)}\left( {x_{k} - x_{k}^{j}} \right)} + {\frac{1}{2}{H\left( {x_{k},x_{k}^{j}} \right)}\left( {x_{k} - x_{k}^{j}} \right)}}},{where}}{{z_{k}^{j} = {E_{j}\left( z_{k} \right)}},{{f^{\prime}(x)} = \left\lbrack \frac{{\partial x_{i}}x_{j}}{\partial x_{l}} \right\rbrack_{i,j,{l = 1},2,\ldots \mspace{14mu},m}},{and}}{{{H\left( {x_{k + 1},x_{k + 1}^{j}} \right)} = \begin{pmatrix}{\left( {x_{k + 1} - x_{k + 1}^{j}} \right)^{T}D_{1}} \\{\left( {x_{k + 1} - x_{k + 1}^{j}} \right)^{T}D_{2}} \\\vdots \\{\left( {x_{k + 1} - x_{k + 1}^{j}} \right)^{T}D_{m}}\end{pmatrix}},{where}}{D = \begin{pmatrix}D_{1} \\\vdots \\D_{m}\end{pmatrix}}} & (9)\end{matrix}$

is the m²×m matrix of second derivatives. Having defined z_(k) ^(j),f′(x) and H(x_(k+1), x_(k+1) ^(j)) the bilinear state space model Kalmanfilter can be completed with the expressions:

{umlaut over (P)} _(k+1) ^(k+1) =P _(k+1) ^(k+1)(V _(k+1)^(k+1))^(T)  (10)

P _(k+1) ^(k+1)=(V _(k+1) ^(k+1)){umlaut over (P)} _(k+1) ^(k+1),  (11)

where the Kalman gain is given by:

$\begin{matrix}{{K_{k + 1} = {P_{k + 1}^{k}{C^{T}\left( {{{CP}_{k + 1}^{k}C^{T}} + R} \right)}^{- 1}}},{and}} & (12) \\{{V_{k + 1}^{k + 1} = {{f^{\prime}\left( x_{k + 1}^{k + 1} \right)} + {\frac{1}{2}{H\left( {x_{k + 1}^{k},x_{k + 1}^{k + 1}} \right)}}}}{{{{for}\mspace{14mu} k} = 0},\ldots \mspace{14mu},{t.}}} & (13)\end{matrix}$

The Kalman smoother is an estimator which operates in the time domainwhere t>k+1. For the interval of k=t−1, . . . , 1, the Kalman smootheris given by:

x _(k) ^(t) =x _(k) ^(k) +J _(k)(x _(k+1) ^(t) −x _(k+1) ^(k))  (14)

P _(k) ^(t) =P _(k) ^(k) +J _(k)(P _(k+1) ^(t) −P _(k+1) ^(k))J _(k)^(T),  (15)

where J_(k)=(P_(k) ^(k)A^(T)+{umlaut over (P)}_(k) ^(k)B^(T))(P_(k+1)^(k))⁻¹. The bilinear lag-one covariance smoother is simply given by:

P _(k+1,k) ^(t) =AP _(k) ^(k) +B({umlaut over (P)} _(k) ^(k))^(T),and  (16)

{umlaut over (P)} _(k+1,k) ^(t) =P _(k+1,k) ^(t)(V _(k) ^(t))^(T).  (17)

As noted above, the term system identification describes themathematical tools and algorithms that build dynamical models frommeasured data. In the following, the bilinear state space model definedby equations (3) and (4) is identified using the expectationmaximization algorithm. The expectation maximization (EM) algorithm isan iterative technique used for obtaining the maximum likelihoodestimation. In the EM algorithm, first q(θ, θ(i))=E{log p(θ, X, Y)|Y} iscomputed, where p(θ, X, Y) is the maximum likelihood function. This isthe E-step. Next, in the M-step, q(θ, θ(i)) is maximized with respect toθ, which is the parameter vector, given by θ={A, B, C, Q, R, V, μ};X={x}₁ ^(t)={x₁, . . . , x_(t)} and Y={y}₁ ^(t)={y₁, . . . , y_(t)}.

For the bilinear state space model of equations (3) and (4), thefollowing assumptions are applied: x₀˜N(μ, V); w_(k)˜N(0, Q); andv_(k)˜N(0, R). Then, q(θ, θ(i)) is given by:

$\begin{matrix}\begin{matrix}{{q\left( {\theta,{\theta (i)}} \right)} = {E\left\{ {\log \; {p\left( {\theta,X,Y} \right)}} \middle| Y \right\}}} \\{= {{{- \frac{1}{2}}\log {V}} - {\frac{1}{2}{Tr}\left\{ {V^{- 1}\left( {\Delta - {{\hat{x}}_{0}\mu^{T}} - {\mu \; {\hat{x}}_{0}^{T}} + {\mu\mu}^{T}} \right)} \right\}}}}\end{matrix} & (18) \\{{{{{- \frac{t}{2}}\log {Q}} - {\frac{1}{2}{Tr}\left\{ {Q^{- 1}\begin{pmatrix}{\Theta - {\Psi \; A^{T}} - {\Pi \; B^{T}} - {A\; \Psi^{T}} +} \\{{A\; \Phi \; A^{T}} - {B\; \Pi^{T}} + {B\; \Lambda \; B^{T}}}\end{pmatrix}} \right\}} - {\frac{t}{2}\log {R}} - {\frac{1}{2}{Tr}\left\{ {R^{- 1}\left( {\delta - {\Omega \; C^{T}} - {C\; \Omega^{T}} + {C\; \Phi \; C^{T}}} \right)} \right\}} + {{const}.}},{{{{where}\mspace{14mu} \Delta} = {E_{t}\left( {x_{0}x_{0}^{T}} \right)}};{{\hat{x}}_{0} = {E_{t}\left( x_{0} \right)}};{\Theta = {\sum\limits_{k = 0}^{t - 1}{E_{t}\left( {x_{k + 1}x_{k + 1}^{T}} \right)}}};}}{{\Phi = {\sum\limits_{k = 0}^{t - 1}{E_{t}\left( {x_{k}x_{k}^{T}} \right)}}};{\Psi = {\sum\limits_{k = 0}^{t - 1}{E_{t}\left( {x_{k + 1}x_{k}^{T}} \right)}}};{\Pi = {\sum\limits_{k = 0}^{t - 1}{E_{t}\left( {x_{k + 1}z_{k}^{T}} \right)}}};}{{\Gamma = {\sum\limits_{k = 0}^{t - 1}{E_{t}\left( {x_{k}z_{k}^{T}} \right)}}};{\Lambda = {\sum\limits_{k = 0}^{t - 1}{E_{t}\left( {z_{k}z_{k}^{T}} \right)}}};{\Omega = {\sum\limits_{k = 0}^{t - 1}{E_{t}\left( {y_{k}x_{k}^{T}} \right)}}};}{{\delta = {\sum\limits_{k = 0}^{t - 1}{E_{t}\; \left( {y_{k}y_{k}^{T}} \right)}}};{{{and}\mspace{14mu} z_{k}} = {x_{k} \otimes {x_{k}.}}}}} & \;\end{matrix}$

The function q(θ, θ(i)), given above in equation (18), is maximized overθ when:

μ = x̂₀; V = Δ; A = Ψ Φ⁻¹; B = Π Λ⁻¹;${Q = {\frac{1}{t}\left( {\Theta - {\Psi^{T}\Phi^{- 1}\Psi} - {{\Pi\Lambda}^{- 1}\Pi^{T}}} \right)}};$C = Ω Φ⁻¹; and  R = t⁻¹{δ − ΩΦ⁻¹Ω^(T)}.

In the following, the above bilinear Kalman model is applied to thenonlinear estimation and identification of the Lotka-Volterra nonlinearmodel. The Lotka-Volterra nonlinear model has wide ranging applicationsin various domains of life science, biology, chemistry, economics andneural networks. In order to show the efficiency and accuracy of thepresent bilinear Kalman filter, the bilinear Kalman filter and smootherare applied to simultaneously estimate states and parameters from noisedata of a Lotka-Volterra system.

The Lotka-Volterra predator-prey equations are a pair of first-order,nonlinear differential equations frequently used to describe thedynamics of biological systems in which two species interact; i.e.,predator and prey. The Lotka-Volterra model has the following form:

$\begin{matrix}{\frac{x}{t} = {{\lambda \; x} - {\beta \; {xy}}}} & (19) \\{{\frac{y}{t} = {{\beta \; {xy}} - {\gamma \; y}}},} & (20)\end{matrix}$

where y represents the number of predators and x represents the numberof the corresponding prey,

$\frac{x}{t}\mspace{14mu} {and}\mspace{14mu} \frac{y}{t}$

represent the growth of the two populations against time t, and λ, β andγ are parameters representing the interaction of the two species.

The Lotka-Volterra competing species model is a system of ordinarydifferential equations of the following form:

$\begin{matrix}{\frac{x}{t} = {{\lambda \; {x\left( {1 - x} \right)}} - {\beta \; {xy}}}} & (21) \\{\frac{y}{t} = {{\gamma \; {y\left( {1 - y} \right)}} - {\beta \; {{xy}.}}}} & (22)\end{matrix}$

Equation (21) indicates that the population of the species x growsaccording to a logistic law in absence of species y (i.e., when y=0).Additionally, the rate of growth of x is negatively proportional to y,representing competition between members of x and members of y. Thelarger the population y, the smaller the growth rate of x. Similarly,equation (22) describes the rate of growth for population y.

The Lotka-Volterra competing species model can be written in the form ofthe present bilinear model (of equations (3) and (4)), with

$A = {{\begin{pmatrix}{\lambda \;} & 0 \\0 & \gamma\end{pmatrix}\mspace{14mu} {and}\mspace{14mu} B} = {\begin{pmatrix}{- \lambda} & {- \beta} & 0 \\0 & {- \beta} & {- \gamma}\end{pmatrix}.}}$

The same can be performed for the Lotka-Volterra predator-prey model ofequations (19) and (20).

The following simulation using the present bilinear Kalman filter andbilinear Kalman smoother was performed on the nonlinear Lotka-Volterracompetition model in state-space form, which is given by:

$\begin{matrix}{x_{k + 1} = {{Ax}_{k} + {Bz}_{k} + w_{k}}} & (23) \\{{{y_{k + 1} = {{Cx}_{k} + v_{k}}},{{{where}\mspace{14mu} A} = \begin{pmatrix}1 & 0.1 \\{- 0.1} & 1\end{pmatrix}},{B = \begin{pmatrix}0.01 & 0.01 & 0 \\0 & {- 0.01} & 0\end{pmatrix}}}{{{{and}\mspace{14mu} C} = \begin{pmatrix}1 & 0 \\0 & 1\end{pmatrix}},}} & (24)\end{matrix}$

and the bilinear term z_(k)=x_(k)

x_(k), where x_(k) is the state vector. The random noise w_(k) and v_(k)are uncorrelated with w_(k)˜(0, W) and v_(k)˜(0, V), where W=0.0004I₂and V=0.0004I₂. The initial state is

$x_{0} = {\begin{pmatrix}1 \\1\end{pmatrix}.}$

The error for the estimated quantities is required in order to state thereliability of the results. The error is provided by the covariancematrix to compute the difference between the true states and theestimated states. In other words, if x_(k) is the true state, and x_(k)^(t) is the estimated state, then the estimation error can be computedas:

ε=∥x _(k) −x _(k) ^(t)∥=(x _(k) −x _(k) ^(t))^(T)(x _(k) −x _(k)^(t)).  (25)

In the bilinear Kalman filter, the estimated state is x_(k+1). The valueof x_(k+1) ^(k) can be obtained from equation (5). Having run thesimulation with the present Kalman filter and Kalman smoother, it isfound that the error between the true states and the estimated statesvia the bilinear Kalman filter is very small. The error in the case ofthe bilinear filter is shown in FIG. 2.

In the bilinear Kalman smoother, the estimated state is x_(k+1) ^(t) fort>k+1. The estimated state for the Lotka-Volterra model can be computedby using the bilinear Kalman smoother of equation (14). By applying theequations of this estimator to estimate the state x_(j), the errorbetween the true state and the estimated state is found to be verysmall, as shown in FIG. 3. These results show that the present bilinearKalman filter and bilinear Kalman smoother work well and are applicableto a bilinear model.

The utility of the bilinear Kalman filter and bilinear Kalman smootherhave also been simulated for estimation of bilinear system parametersvia the EM approach. In this simulation, the Lotka-Volterra state spacemodel is, once again, considered, with the parameters A, B, C, W and Vbeing unknown. The initial value for the state in this simulation is

${x_{0} = \begin{pmatrix}1 \\1\end{pmatrix}},$

such that x₀˜N(μ, P). The additive noise w_(k) and v_(k) areuncorrelated with w_(k)˜(0, W) and v_(k)˜(0, V), with initial estimatesof W and V. The estimation for the system of the model given byequations (23) and (24) is determined from this information about themodel via the EM algorithm. The simulation begins with initial guessesfor such parameters, and these are updated recursively until convergenceto the true system; i.e., if the estimated parameters are very close tothe true parameters, then a small error is obtained between theestimated state and the true state. The E-step and M-step are bothoutlined above.

FIG. 4 shows the error between the true state and the estimated stateusing the estimated values of the parameters that were obtained from thesimulation via the bilinear EM algorithm. The resulting small errorsindicate that using the bilinear EM approach to estimate the exactvalues of the parameters is very reliable.

The extended Kalman filter (EKF) and the ensemble Kalman filter (EnKF)are conventional prior art filters used for nonlinear models.Application of these filters to the nonlinear Lotka-Volterra modelproduces relatively good results. However, when compared against thepresent bilinear Kalman filter, the error between the true states andthe estimated states is found to be smaller with the present bilinearKalman filter than either of the prior art filters. The error betweenthe true states and the estimated states for the present bilinear Kalmanfilter is shown in FIG. 5. The same error for the extended Kalman filteris shown in FIG. 6, and the error for the ensemble Kalman filter isshown in FIG. 7. Comparing the errors, the error is found to be smallestwith the present bilinear Kalman filter, thus showing that the bilinearKalman filter produces more accurate results than either conventionalprior art variant on the Kalman filter.

It should be understood that the calculations of state observer 100 maybe performed by any suitable computer system, such as thatdiagrammatically shown in FIG. 1. Data is fed into system 100 by sensors202, and may further be input via any suitable type of user interface116, and may be stored in memory 112, which may be any suitable type ofcomputer readable and programmable memory. Calculations are performed byprocessor 114, which may be any suitable type of computer processor andare fed, as control signals, to controls 208. Controls 208 may alsoinclude a display for the user, which may be any suitable type ofcomputer display.

Processor 114 may be associated with, or incorporated into, any suitabletype of computing device, for example, a personal computer or aprogrammable logic controller. The controls 208, the processor 114, thememory 112 and any associated computer readable recording media are incommunication with one another by any suitable type of data bus, as iswell known in the art.

Examples of computer-readable recording media include a magneticrecording apparatus, an optical disk, a magneto-optical disk, and/or asemiconductor memory (for example, RAM, ROM, etc.). Examples of magneticrecording apparatus that may be used in addition to memory 112, or inplace of memory 112, include a hard disk device (HDD), a flexible disk(FD), and a magnetic tape (MT). Examples of the optical disk include aDVD (Digital Versatile Disc), a DVD-RAM, a CD-ROM (Compact Disc-ReadOnly Memory), and a CD-R (Recordable)/RW.

Alternatively, the present Kalman filter may be implemented by a digitalsignal processor, a microcontroller, an application specific integratedcircuit (ASIC), or any other suitable circuit or device programmed orconfigured to carry out the steps of the method.

It is to be understood that the present invention is not limited to theembodiments described above, but encompasses any and all embodimentswithin the scope of the following claims.

We claim:
 1. A method for Kalman filter state estimation in bilinearsystems, comprising the steps of: (a) generating an observer in anelectronic device for estimating a state vector x_(k) associated with abilinear system, the observer defining a covariance matrix P_(k)statistically relating the state vector x_(k), where k represents aninteger time increment, the bilinear system being defined byx_(k+1)=Ax_(k)+B(x_(k)

x_(k))+w_(k) where A is a transition matrix defined by the bilinearsystem, B is a measurement matrix defined by parameters of the bilinearsystem, and w_(k) is system noise at time k; (b) receiving sensor datay_(k) at the electronic device from a sensor, the sensor data beingassociated with the state vector x_(k) as y_(k)=Cx_(k)+v_(k), where C isa measurement matrix defined by the parameters of the bilinear systemand v_(k) is measurement noise at time k; (c) generating initialestimates of the state vector and the covariance matrix from the sensordata and storing the initial estimates of the state vector and thecovariance matrix in non-transitory computer readable memory of theelectronic device; (d) calculating a projected state vector x_(k+1) ^(k)representing the state vector projected ahead by one time increment asx_(k+1) ^(k)=Ax_(k) ^(k)+Bz_(k) ^(k) and storing the projected statevector x_(k+1) ^(k) in the non-transitory computer readable memory ofthe electronic device, where z_(k)=x_(k)

x_(k) and z_(k) ^(k)=E_(k)(z_(k)); (e) calculating a projectedcovariance matrix P_(k+1) ^(k) representing the covariance matrixprojected ahead by one time increment as P_(k+1) ^(k)=AP_(k)^(k)A^(T)+A{umlaut over (P)}_(k) ^(k)B^(T)+B({umlaut over (P)}_(k)^(k))^(T)A^(T)+B{dot over (P)}_(k) ^(k)B^(T)+Q, where Q represents acovariance of the system noise w_(k), and storing the projectedcovariance matrix P_(k+1) ^(k) in the non-transitory computer readablememory of the electronic device; (f) calculating a Kalman gain K_(k+1)as K_(k+1)=P_(k+1) ^(k)C^(T)(CP_(k+1) ^(k)C^(T)+R)⁻¹, where R is acovariance of the measurement noise v_(k), and storing the Kalman gainK_(k+1) in the non-transitory computer readable memory of the electronicdevice; (g) receiving a new set of measurement data y_(k) from thesensor and updating the state vector and the covariance matrix basedupon the new set of measurement data as x_(k+1) ^(k+1)=x_(k+1)^(k)+K_(k+1)(y_(k)−Cx_(k+1) ^(k)) and P_(k+1) ^(k+1)=(I−K_(k+1)C)P_(k+1)^(k), respectively, wherein x_(k+1) ^(k+1) represents the updated statevector for time (k+1) and P_(k+1) ^(k+1) represents the updatedcovariance matrix for time (k+1), where I represents the identitymatrix, and storing the updated state vector and the updated covariancematrix in the non-transitory computer readable memory of the electronicdevice; and (h) returning to steps (d) and (e) with the updated statevector and the updated covariance matrix, respectively.
 2. The methodfor Kalman filter state estimation in bilinear systems as recited inclaim 1, further comprising the step of calculating a further updatedstate vector x_(k) ^(t) as x_(k) ^(t)=x_(k) ^(k)+J_(k)(x_(k+1)^(t)−x_(k+1) ^(k)) and a further updated covariance matrix P_(k) ^(t) asP_(k) ^(t)=P_(k) ^(k)+J_(k)(P_(k+1) ^(t)−P_(k+1) ^(k))J_(k) ^(T) for atime t, where t>k+1 in an interval k=t−1, . . . , 1, following the step(g) and prior to the step (h), wherein J_(k)=(P_(k) ^(k)A^(T)+{umlautover (P)}_(k) ^(k)B^(T))(P_(k+1) ^(k))⁻¹.
 3. A system for Kalman filterstate estimation in bilinear systems, comprising: a processor;non-transitory computer readable memory coupled to the processor;software stored in the non-transitory computer readable memory andexecutable by the processor, the software having: means for generatingan observer for estimating a state vector x_(k) associated with abilinear system, the observer defining a covariance matrix P_(k)statistically relating the state vector x_(k), where k represents aninteger time increment, the bilinear system being defined byx_(k+1)=Ax_(k)+B(x_(k)

x_(k))+w_(k) where A is a transition matrix defined by the bilinearsystem, B is a measurement matrix defined by parameters of the bilinearsystem, and w_(k) is system noise at time k; means for receiving sensordata y_(k) from a sensor, the sensor data being associated with thestate vector x_(k) as y_(k)=Cx_(k)+v_(k), where C is a measurementmatrix defined by the parameters of the bilinear system and v_(k) ismeasurement noise at time k; means for generating initial estimates ofthe state vector and the covariance matrix from the sensor data andstoring the initial estimates of the state vector and the covariancematrix in the non-transitory computer readable memory; means forcalculating a projected state vector x_(k+1) ^(k) representing the statevector projected ahead by one time increment as x_(k+1) ^(k)=Ax_(k)^(k)+Bz_(k) ^(k) and storing the projected state vector x_(k+1) ^(k) inthe non-transitory computer readable memory, where z_(k)=x_(k)

x_(k) and z_(k) ^(k)=E_(k)(z_(k)); means for calculating a projectedcovariance matrix P_(k+1) ^(k) representing the covariance matrixprojected ahead by one time increment as P_(k+1) ^(k)=AP_(k)^(k)A^(T)+A{umlaut over (P)}_(k) ^(k)B^(T)+B({umlaut over(P)}k^(k))^(T)A^(T)+B{dot over (P)}_(k) ^(k)B^(T)+Q, where Q representsa covariance of the system noise w_(k), and storing the projectedcovariance matrix P_(k+1) ^(k) in the non-transitory computer readablememory; means for calculating a Kalman gain K_(k+1) as K_(k+1)=P_(k+1)^(k)C^(T)(CP_(k+1) ^(k)C^(T)+R)⁻¹, where R is a covariance of themeasurement noise v_(k), and storing the Kalman gain K_(k+1) in thenon-transitory computer readable memory; and means for receiving a newset of measurement data y_(k) from the sensor and updating the statevector and the covariance matrix based upon the new set of measurementdata as x_(k+1) ^(k+1)=x_(k+1) ^(k)+K_(k+1)(y_(k)−Cx_(k+1) ^(k)) andP_(k+1) ^(k+1)=(I−K_(k+1)C)P_(k+1) ^(k), respectively, wherein x₊₁^(k+1) represents the updated state vector for time (k+1) and P_(k+1) ⁺¹represents the updated covariance matrix for time (k+1), where Irepresents the identity matrix, and storing the updated state vector andthe updated covariance matrix in the non-transitory computer readablememory.
 4. The system for Kalman filter state estimation in bilinearsystems as recited in claim 3, further comprising means for calculatinga further updated state vector x_(k) ^(t) as x_(k) ^(t)=x_(k)^(k)+J_(k)(x_(k+1) ^(t)−x_(k+1) ^(k)) and a further updated covariancematrix P_(k) ^(t) as P_(k) ^(t)=P_(k) ^(k)+J_(k)(P_(k+1) ^(t)−P_(k+1)^(k))J_(k) ^(T) for a time t, where t>k+1 in an interval k=t−1, . . . ,1, wherein J_(k)=(P_(k) ^(k)A^(T)+{umlaut over (P)}_(k)^(k)B^(T))(P_(k+1) ^(k))⁻¹.
 5. A computer software product that includesa non-transitory computer readable storage medium readable by aprocessor, the non-transitory computer readable storage medium havingstored thereon a set of instructions for Kalman filter state estimationin bilinear systems, the instructions comprising: (a) a first sequenceof instructions which, when executed by the processor, causes theprocessor to generate an observer for estimating a state vector x_(k)associated with a bilinear system, the observer defining a covariancematrix P_(k) statistically relating the state vector x_(k), where krepresents an integer time increment, the bilinear system being definedby x_(k+1)=Ax_(k)+B(x_(k)

x_(k))+w_(k) where A is a transition matrix defined by the bilinearsystem, B is a measurement matrix defined by parameters of the bilinearsystem, and w_(k) is system noise at time k; (b) a second sequence ofinstructions which, when executed by the processor, causes the processorto receive sensor data y_(k) from a sensor, the sensor data beingassociated with the state vector x_(k) as y_(k)=Cx_(k)+v_(k), where C isa measurement matrix defined by the parameters of the bilinear systemand v_(k) is measurement noise at time k; (c) a third sequence ofinstructions which, when executed by the processor, causes the processorto generate initial estimates of the state vector and the covariancematrix from the sensor data and storing the initial estimates of thestate vector and the covariance matrix in the non-transitory computerreadable storage medium; (d) a fourth sequence of instructions which,when executed by the processor, causes the processor to calculate aprojected state vector x_(k+1) ^(k) representing the state vectorprojected ahead by one time increment as x_(k+1) ^(k)=Ax_(k) ^(k)+Bz_(k)^(k) and storing the projected state vector x_(k+1) ^(k) in thenon-transitory computer readable storage medium, where z_(k)=x_(k)

x_(k) and z_(k) ^(k)=E_(k)(z_(k)); (e) a fifth sequence of instructionswhich, when executed by the processor, causes the processor to calculatea projected covariance matrix P_(k+1) ^(k) representing the covariancematrix projected ahead by one time increment as P_(k+1) ^(k)=AP_(k)^(k)A^(T)+A{umlaut over (P)}_(k) ^(k)B^(T)+B({umlaut over (P)}_(k)^(k))^(T)A^(T)+B{dot over (P)}_(k) ^(k)B^(T)+Q, where Q represents acovariance of the system noise w_(k), and storing the projectedcovariance matrix P_(k+1) ^(k) in the non-transitory computer readablestorage medium; (f) a sixth sequence of instructions which, whenexecuted by the processor, causes the processor to calculate a Kalmangain K_(k+1) as K_(k+1)=P_(k+1) ^(k)C^(T)(CP_(k+1) ^(k)C^(T)+R)⁻¹, whereR is a covariance of the measurement noise v_(k), and storing the Kalmangain K_(k+1) in the non-transitory computer readable storage medium; (g)a seventh sequence of instructions which, when executed by theprocessor, causes the processor to receive a new set of measurement datay_(k) from the sensor and update the state vector and the covariancematrix based upon the new set of measurement data as x_(k+1)^(k+1)=x_(k+1) ^(k)+K_(k+1)(y_(k)−Cx_(k+1) ^(k)) and P_(k+1)^(k+1)=(I−K_(k+1)C)P_(k+1) ^(k), respectively, wherein x_(k+1) ^(k+1)represents the updated state vector for time (k+1) and P_(k+1) ^(k+1)represents the updated covariance matrix for time (k+1), where Irepresents the identity matrix, and storing the updated state vector andthe updated covariance matrix in the non-transitory computer readablestorage medium; and (h) an eighth sequence of instructions which, whenexecuted by the processor, causes the processor to return to the fourthand fifth sequences of instructions with the updated state vector andthe updated covariance matrix, respectively.
 6. The computer softwareproduct as recited in claim 5, further comprising a ninth sequence ofinstructions which, when executed by the processor, causes the processorto calculate further updated state vector x_(k) ^(t) as x_(k) ^(t)=x_(k)^(k)+J_(k)(x_(k+1) ^(t)−x_(k+1) ^(k)) and a further updated covariancematrix P_(k) ^(t) as P_(k) ^(t)=P_(k) ^(k)+J_(k)(P_(k+1) ^(t)−P_(k+1)^(k))J_(k) ^(T) for a time t, where t>k+1 in an interval k=t−1, . . . ,1, following the seventh set of instructions and prior to the eighth setof instructions, wherein J_(k)=(P_(k) ^(k)A^(T)+{umlaut over (P)}_(k)^(k)B^(T))(P_(k+1) ^(k))⁻¹.